﻿using System;
using System.Collections.Generic;
using UnityEngine;

namespace Pathfinding
{
    /// <summary>
    /// A星搜索器
    /// </summary>
    /// <typeparam name="TMap">搜索的图类</typeparam>
    /// <typeparam name="TNode">搜索的节点类</typeparam>
    public class AStarSearcher<TMap, TNode> where TNode : class, IAStarNode<TNode>, IComparable<TNode>
    {
        private readonly HashSet<TNode> _closeList; //探索集
        private readonly Heap<TNode> _openList; //边缘集
        private readonly TMap _nodeMap; //搜索空间（地图）

        public AStarSearcher(TMap map, int maxNodeSize = 200)
        {
            _nodeMap = map;
            _closeList = new HashSet<TNode>();
            //maxNodeSize用于限制路径节点的上限，避免陷入无止境搜索的情况
            _openList = new Heap<TNode>(maxNodeSize);
        }

        /// <summary>
        /// 搜索（寻路）
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="target">终点</param>
        /// <param name="pathRes">返回生成的路径</param>
        public void FindPath(TNode start, TNode target, Stack<TNode> pathRes)
        {
            pathRes.Clear(); //清空路径以备存储新的路径
            _closeList.Clear();
            _openList.Clear();
            _openList.PushHeap(start);
            while (!_openList.IsEmpty)
            {
                var currentNode = _openList.Top;
                _openList.PopHeap();
                _closeList.Add(currentNode); //拟定移动到该节点，将其放入探索集
                if (currentNode.Equals(target) || _openList.IsFull) //如果找到了或图都搜完了也没找到时
                {
                    GenerateFinalPath(start, target, pathRes); //生成路径并保存到pathRes中
                    return;
                }

                UpdateList(currentNode, target); //更新边缘集和探索集
            }

            return;
        }

        private void GenerateFinalPath(TNode startNode, TNode endNode, Stack<TNode> pathStack)
        {
            pathStack.Push(endNode); //因为回溯，所以用栈储存生成的路径
            var tpNode = endNode.Parent;
            while (!tpNode.Equals(startNode))
            {
                pathStack.Push(tpNode);
                tpNode = tpNode.Parent;
            }

            pathStack.Push(startNode);
        }

        private void UpdateList(TNode curNode, TNode endNode)
        {
            var successors = curNode.GetSuccessors(_nodeMap); //找出当前节点的后继节点
            foreach (var sucNode in successors)
            {
                if (_closeList.Contains(sucNode))
                    continue;
                var tpCost = curNode.GCost + sucNode.SelfCost;
                var isNotInOpenList = !_openList.Contains(sucNode);
                if (!isNotInOpenList && !(tpCost < sucNode.GCost)) continue;
                sucNode.GCost = tpCost;
                sucNode.HCost = sucNode.GetDistance(endNode); //计算启发函数估计值
                sucNode.Parent = curNode; //记录父节点，方便回溯
                if (isNotInOpenList)
                {
                    _openList.PushHeap(sucNode);
                }
            }
        }
    }
}